#include <iostream>
#include <fstream>
#include <string>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <vector>
#include <string.h>



#include "cv.h"
#include "highgui.h"
#include <opencv.hpp>



//#include "ros/ros.h"

#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/features/integral_image_normal.h>
#include <pcl/visualization/cloud_viewer.h>



//#include <sensor_msgs/PointCloud2.h>
//#include <std_msgs/Int32.h>

#include <Eigen/Core> 
#include <Eigen/Geometry>

#include "Frame_input.h"



#include "graph/VertexCamera.h"
#include "graph/VertexPlane.h"
#include "graph/VertexPoint3D.h"
#include "graph/EdgeCameraPlane.h"



#include "FeatureDescriptor.h"
#include "SurfFeatureDescriptor64.h"
#include "SurfFeatureDescriptor128.h"
//#include "Histogram.cpp"
//#include "RGBFeatureDescriptor.cpp"
#include "OrbFeatureDescriptor.h"



#include "Point.h"
#include "Plane.h"
#include "Line.h"
#include "KeyPoint.h"
#include "KeyPointSet.h"


#include "Association.h"
#include "KeyPointAssociation.h"
#include "PlaneAssociation.h"
#include "AssociationSet.h"

#include "FeatureExtractor.h"
#include "SurfExtractor.h"
#include "OrbExtractor.h"

#include "Map3D.cpp"
#include "RGBDFrame.cpp"
#include "Transformation.cpp"

#include "FeatureExtractor.cpp"
#include "SurfExtractor.cpp"
#include "OrbExtractor.cpp"



#include "FrameMatcher.cpp"
#include "DummyMatcher.cpp"
#include "SurfRansacMatcherI.cpp"
#include "JCMergeMatcherI.cpp"
#include "JCSearchMatcherI.cpp"
#include "BasicIcpMatcher.cpp"
#include "BasicGIcpMatcher.cpp"
#include "NDTMatcher.cpp"
#include "KeyPointIcpMatcherI.cpp"
#include "AICK.cpp"
#include "PlanePostProcessMatcher.cpp"
#include "SACIAMatcher.cpp"

#include <pthread.h>
#include <sys/types.h>
#include <dirent.h>
#include <errno.h>

using namespace std;
using namespace Eigen;
//string out_path = "/media/2A98195B9819273B/output_ssd/output/output/";
string out_path = "./output/";

int getdir (string dir, vector<string> &files)
{
    DIR *dp;
    struct dirent *dirp;
    if((dp  = opendir(dir.c_str())) == NULL) {
        cout << "Error(" << errno << ") opening " << dir << endl;
        return errno;
    }

    while ((dirp = readdir(dp)) != NULL) {
        files.push_back(string(dirp->d_name));
    }
    closedir(dp);
    return 0;
}

int main(int argc, char **argv)
{
	//boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
	//viewer->setBackgroundColor (0.5f, 0.5f, 0.5f);
	//viewer->addCoordinateSystem (1.0);
	//viewer->initCameraParameters ();
	
	Map3D * map = new Map3D();
	//map->setVisualization(viewer);
	FeatureExtractor * orb = new OrbExtractor();
	SurfExtractor * surf = new SurfExtractor();
	surf->thres			= 0.0000003f;
	
	PlanePostProcessMatcher * frame_matcher = new PlanePostProcessMatcher();
	SACIAMatcher * sacia = new SACIAMatcher();
	NDTMatcher * ndtmatcher = new NDTMatcher();
	AICK * aick = new AICK();
	aick->nr_iter = 20;
	aick->shrinking = 0.85f;
	aick->max_points = 2000;
	
	vector< FrameMatcher * > test_matchers;
	test_matchers.push_back(aick);
	//test_matchers.push_back(sacia);
	for(int i = 0; i < 10; i++){
		AICK * aickTest = new AICK();
		aickTest->nr_iter = 20;
		aickTest->shrinking = 0.85f;
		aickTest->max_points = 100*(i+1);
		//test_matchers.push_back(aickTest);
	}
	
	frame_matcher->base = aick;
	map->matcher = ndtmatcher;
	map->extractor = surf;

	string path 		= "/home/johane/test_data/rgbd_dataset_freiburg1_room/";
	string path_gt		= path+"groundtruth.txt";
	string path_rgb		= path+"rgb.txt";
	string path_depth	= path+"depth.txt";
	string path_asso	= path+"asso_w_gt.txt";
	
	Calibration * calib = new Calibration();
	calib->fx			= 517.3;
	calib->fy			= 516.5;
	calib->cx			= 318.6;
	calib->cy			= 255.3;
	calib->ds			= 1.035;
	calib->scale		= 5000;
	
	segmenter = new RGBDSegmentation(calib);
	//map->seg = seg;
	
	orb->calibration = calib;
	surf->calibration = calib;

	string line;
	ifstream asso_file (path_asso.c_str());
	vector<Frame_input *> frame_inputs;
	vector<Eigen::Matrix4f> frame_gt;
	if (asso_file.is_open()){
		while ( asso_file.good() )
		{
			getline (asso_file,line);
			if(line[0] != '#'){
				int space1 = line.find(" ");
				if(space1 != -1){
					//printf("%s\n",line.c_str());
					
					Frame_input * fi =  new Frame_input();
					int space2  = line.find(" ",space1+1,1);
					int space3  = line.find(" ",space2+1,1);
					int space4  = line.find(" ",space3+1,1);
					
					int space5  = line.find(" ",space4+1,1);
					int space6  = line.find(" ",space5+1,1);
					int space7  = line.find(" ",space6+1,1);
					int space8  = line.find(" ",space7+1,1);
					int space9  = line.find(" ",space8+1,1);
					int space10 = line.find(" ",space9+1,1);
					int space11 = line.find(" ",space10+1,1);
					int space12 = line.find(" ",space11+1,1);

					fi->depth_timestamp		= atof(line.substr(0,space1).c_str());
					fi->depth_path			= path+line.substr(space1+1,space2-space1-1).c_str();
					fi->rgb_timestamp		= atof(line.substr(space2+1,space3-space2-1).c_str());
					fi->rgb_path 			= path+line.substr(space3+1,space4-space3-1).c_str();
					fi->calibration			= calib;
					frame_inputs.push_back(fi);
					//tx ty tz qx qy qz qw
					double ts			= atof(line.substr(space4+1,space5-space4-1).c_str());
					double tx			= atof(line.substr(space5+1,space6-space5-1).c_str());
					double ty			= atof(line.substr(space6+1,space7-space6-1).c_str());
					double tz			= atof(line.substr(space7+1,space8-space7-1).c_str());
					double qx			= atof(line.substr(space8+1,space9-space8-1).c_str());
					double qy			= atof(line.substr(space9+1,space10-space9-1).c_str());
					double qz			= atof(line.substr(space10+1,space11-space10-1).c_str());
					double qw			= atof(line.substr(space11+1,space12-space11-1).c_str());
					//printf("Timestamp: %f -> %.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f\n",gt[0],gt[1],gt[2],gt[3],gt[4],gt[5],gt[6],gt[7]);
					g2o::SE3Quat poseSE3 (Eigen::Quaterniond(qw,qx,qy,qz), Eigen::Vector3d(tx,ty,tz));
					Eigen::Matrix4f mat = Eigen::Matrix4f::Identity();//poseSE3.to_homogenious_matrix().cast<float>();

					//Eigen::Quaternionf quat(qw,qx,qy,qz);
					Eigen::Quaternionf quat(qw,qx,qy,qz);
					//quat.w() = qw;
					//quat.x() = qx;
					//quat.y() = qy;
					//quat.z() = qz;
					printf("%f %f %f %f\n",quat.w(),quat.x(),quat.y(),quat.z());
					Eigen::Matrix3f rotmat = Eigen::Matrix3f(quat);
					//mat = mat*quat;
					mat(0,3)=tx;
					mat(1,3)=ty;
					mat(2,3)=tz;
					mat(0,0)=rotmat(0,0);
					mat(0,1)=rotmat(0,1);
					mat(0,2)=rotmat(0,2);
					
					mat(1,0)=rotmat(1,0);
					mat(1,1)=rotmat(1,1);
					mat(1,2)=rotmat(1,2);
					
					mat(2,0)=rotmat(2,0);
					mat(2,1)=rotmat(2,1);
					mat(2,2)=rotmat(2,2);
					frame_gt.push_back(mat);
				}
			}
		}
		asso_file.close();
	}else{cout << "Unable to open " << path_asso;}
	

	vector<RGBDFrame *> test_frames;
	for(int i = 40; i < frame_inputs.size() && i < 60;i++)
	{
		RGBDFrame * current_frame = new RGBDFrame(frame_inputs.at(i),map);
		test_frames.push_back(current_frame);
		//map->addFrame(current_frame);
	}
	//map->estimate();
	
	pcl::visualization::CloudViewer viewer("Simple Cloud Viewer");
	

	for(int i = 0; i < test_frames.size(); i++){

		for(int j = i+10; j < test_frames.size(); j++){
			RGBDFrame * src = test_frames.at(i);
			RGBDFrame * dst = test_frames.at(j);

			
//# timestamp tx ty tz qx qy qz qw
			//g2o::SE3Quat src_poseSE3 (Eigen::Quaterniond(gt_i[7], gt_i[4], gt_i[5], gt_i[6]), Eigen::Vector3d(gt_i[1], gt_i[2], gt_i[3]));
			//g2o::SE3Quat dst_poseSE3 (Eigen::Quaterniond(gt_j[7], gt_j[4], gt_j[5], gt_j[6]), Eigen::Vector3d(gt_j[1], gt_j[2], gt_j[3]));
				
			Eigen::Matrix4f src_mat = frame_gt.at(i);
			Eigen::Matrix4f dst_mat = frame_gt.at(j);
			

			Eigen::Matrix4f true_trans_mat1 = src_mat.inverse()*dst_mat;
			Eigen::Matrix4f true_trans_mat2 = dst_mat.inverse()*src_mat;
			Eigen::Matrix4f true_trans_mat3 = src_mat*dst_mat.inverse();
			Eigen::Matrix4f true_trans_mat4 = dst_mat*src_mat.inverse();
			
			Transformation * gt_transformation = new Transformation();
			gt_transformation->transformationMatrix = true_trans_mat2;
			gt_transformation->src = src;
			gt_transformation->dst = dst;
			gt_transformation->weight = 100;
			
			printf("---------------------------------------------------\n");
			cout<<"gt:\n"<<true_trans_mat1<<endl;
			cout<<"gt:\n"<<true_trans_mat2<<endl;
			cout<<"gt:\n"<<true_trans_mat3<<endl;
			cout<<"gt:\n"<<true_trans_mat4<<endl;
			//cout<<"src_gt:\n"<<src_mat<<endl;
			//cout<<"dst_gt:\n"<<dst_mat<<endl;
			gt_transformation->show(&viewer);
			

			
			for(int k = 0; k < test_matchers.size(); k++){
				Transformation * ret = test_matchers.at(k)->getTransformation(src,dst);
				
				float sum1 = 0;
				float sum2 = 0;
				float sum3 = 0;
				float sum4 = 0;
				for(int ll = 0; ll < 4; ll++)
				{
					for(int mm = 0; mm < 4; mm++)
					{
						sum1+=fabs(ret->transformationMatrix(ll,mm)-true_trans_mat1(ll,mm));
						sum2+=fabs(ret->transformationMatrix(ll,mm)-true_trans_mat2(ll,mm));
						sum3+=fabs(ret->transformationMatrix(ll,mm)-true_trans_mat3(ll,mm));
						sum4+=fabs(ret->transformationMatrix(ll,mm)-true_trans_mat4(ll,mm));
					}
				}
				cout<<"est1: "<<sum1<<endl;
				cout<<"est2: "<<sum2<<endl;
				cout<<"est3: "<<sum3<<endl;
				cout<<"est4: "<<sum4<<endl;
				//cout<<"est inv:\n"<<ret->transformationMatrix.inverse()<<endl;
				//ret->show(&viewer);
			}
		}
	}
	return 0;
}
